00100	ENTRY COURSE,DRIVE,INITIAL;
00200	BEGIN "ROVER"
00300	
00400	INTERNAL REAL ARRAY MCL,MVL,EVL,ECL[1:4,1:3];
00500	INTERNAL REAL PAN,TILT,WHEELS,
00600		AARCX,	AARCY,	AARCZ,
00700		BARCX,	BARCY,	BARCZ,
00800		COTX,	COTY,	COTZ;
00900	
01000		DEFINE α="COMMENT";
01050	REQUIRE "SAITRG[SYS,BGB]" LOAD_MODULE;
01100	EXTERNAL REAL PROCEDURE SIN(REAL X);
01200	EXTERNAL REAL PROCEDURE COS(REAL X);
01300	EXTERNAL REAL PROCEDURE ASIN(REAL X);
01400	EXTERNAL REAL PROCEDURE ACOS(REAL X);
01500	EXTERNAL REAL PROCEDURE SQRT(REAL X);
01600	
01700		REAL ARRAY C[1:4,1:3];		α CAMERA LOCUS IN VEHICLE COORDINATES;
01800		REAL CP,SP,CT,ST,		α COS & SIN OF PAN & TILT;
01900			∂Z,			α VERTICAL DISPLACEMENT QUANTA;
02000			CCC,SSS,		α COS & SIN OF TURN QUANTA;
02100			CCW,			α COUNTER CLOCKWISE SIGN;
02200			R,			α RADIUS OF TURN;
02225			SPEED,			α IN FEET PER SECOND;
02237			TIMDEL,			α QUANTUM OF RUNTIME FOR DRIVING;
02250			TOLENS,TOPIVOT,TODECK,CARTLENGTH;	α CART'S PHYSICAL DIMENSIONS;
02300	INTEGER I,N;
02400	
02500	PROCEDURE TTYARRY(STRING S;REAL ARRAY A);
02600	BEGIN
02700		INTEGER I,J;
02800		OUTSTR(13&10&S&13&10);
02900		FOR I←1 STEP 1 UNTIL 4 DO BEGIN
03000		FOR J←1 STEP 1 UNTIL 3 DO
03100		OUTSTR("	"&CVG(A[I,J]) );
03200		OUTSTR(13&10) END;
03300	END;
     

00100	α COMPUTE THE CAMERA LOCUS IN VEHICLE COORDINATES;
00200	PROCEDURE SUBCVC;
00300	BEGIN
00400		REAL CP,SP,CT,ST;		α COS & SIN OF PAN & TILT;
00500		CP	←	COS(PAN);
00600		SP	←	SIN(PAN);
00700		CT	←	COS(TILT);
00800		ST	←	SIN(TILT);
00900		C[1,1]←CP;		C[1,2]←SP;		C[1,3]←0;
01000		C[2,1]←CT*SP;		C[2,2]←-CT*CP;		C[2,3]←-ST;
01100		C[3,1]←-ST*SP;		C[3,2]←ST*CP;		C[3,3]←-CT;
01300		C[4,1]←TOLENS*ST*SP;	C[4,2]←-TOLENS*ST*CP;	C[4,3]←TOLENS*CT+TOPIVOT;
01400	END;
01500	
01600	α COMPUTE THE VEHICLE LOCUS IN CAMERA COORDINATES;
01700	PROCEDURE SUBVCC;
01800	BEGIN
01900		REAL X,Y,Z;
02000		INTEGER I;
02100		SUBCVC;
02200		C[2,1]  ↔  C[1,2];
02300		C[3,1]  ↔  C[1,3];
02400		C[3,2]  ↔  C[2,3];
02500		X	←  -C[4,1];
02600		Y	←  -C[4,2];
02700		Z	←  -C[4,3];
02800		FOR I←1 STEP 1 UNTIL 3 DO C[4,I]←X*C[1,I] + Y*C[2,I] + Z*C[3,I];
02900	END;
03000	
03100	α ROTATE THEN TRANSLATE C BY FRAME X INTO Y;
03200	PROCEDURE ROTTRAN	(REAL ARRAY Y,X);
03300	BEGIN
03400		INTEGER I,J;
03500		FOR I←1 STEP 1 UNTIL 4 DO	α ITH ROW OF C;
03600		FOR J←1 STEP 1 UNTIL 3 DO	α JTH COL OF X;
03700			Y[I,J]	←  C[I,1]*X[1,J] + C[I,2]*X[2,J] + C[I,3]*X[3,J];
03800		FOR J←1 STEP 1 UNTIL 3 DO
03900			Y[4,J]  ←  Y[4,J] + X[4,J];
04000	END;
04100	
04200	PROCEDURE VEHICLE_TO_CAMERA(REAL ARRAY VEHICLE,CAMERA);
04300	BEGIN
04400		SUBCVC;
04500		ROTTRAN(CAMERA,VEHICLE);
04600	END;
04700	
04800	PROCEDURE CAMERA_TO_VEHICLE(REAL ARRAY CAMERA,VEHICLE);
04900	BEGIN
05000		SUBVCC;
05100		ROTTRAN(VEHICLE,CAMERA);
05200	END;
     

00100	α PLACE CART'S MVL AT POINT A FACING ALONG THE COURSE;
00200	INTERNAL PROCEDURE INITIAL;
00300	BEGIN
00301		TOLENS	←	10/12;
00302		TOPIVOT ←	23/12;
00303		TODECK	←	27/12;
00304		CARTLENGTH←	3;
00400		MVL[4,1]	←	AARCX;
00500		MVL[4,2]	←	AARCY;
00600		MVL[4,3]	←	AARCZ + TODECK;
00700		MVL[2,2] ←  MVL[1,1] ← CCW*(AARCX - COTX)/R ;
00800		MVL[2,1] ←-(MVL[1,2] ← CCW*(AARCY - COTY)/R);
00900		MVL[1,3] ← MVL[2,3] ← MVL[3,1] ← MVL[3,2] ← 0;
01000		MVL[3,3] ← 1;
01100		PAN ← WHEELS ← CCW*ASIN(CARTLENGTH/(2*R));
01200		VEHICLE_TO_CAMERA(MVL,MCL);
01410		ARRBLT(EVL[1,1],MVL[1,1],12);
01420		VEHICLE_TO_CAMERA(EVL,ECL);
01700	END;
     

00100	INTERNAL PROCEDURE COURSE;
00200	BEGIN
00300		REAL AX,AY,BX,BY,RR,PHIMAX,ARCLENGTH,PHIDELTA;
00400		I		←	0;
00450		SPEED		←	1;
00475		TIMDEL		←	1;
00500		AX		←	AARCX - COTX;
00600		AY		←	AARCY - COTY;
00700		BX		←	BARCX - COTX;
00800		BY		←	BARCY - COTY;
00900		RR		←	AX↑2 + AY↑2;
01000		R		←	SQRT(RR);
01100		PHIMAX 		←	ACOS((AX*BX + AY*BY)/RR);
01200		ARCLENGTH	←	R*PHIMAX;
01300		N		←	ARCLENGTH / (SPEED*TIMDEL);
01400		PHIDELTA	←	PHIMAX/N;
01500		∂Z		←	(BARCZ - AARCZ)/N;
01600		CCW		←	(IF(AX*BY - BX*AY)>0 THEN 1 ELSE -1);
01700		CCC		←	COS(PHIDELTA);
01800		SSS		←	CCW*SIN(PHIDELTA);
01900	END;
     

00100	α Ok now, let's attempt to drive the motherfucking cart from the last known
00200	  Measured Vehicle Locus to an Estimated Vehicle Locus along the course;
00300	PROCEDURE SUBDRIVE;
00400	BEGIN
00500		REAL XX,YY,RX,RY;
00600		XX	←	MVL[4,1] - COTX;
00700		YY	←	MVL[4,2] - COTY;
00800		EVL[4,3]	←	MVL[4,3] + ∂Z;
00900		RX		←	 CCC*XX - SSS*YY;
01000		RY		←	 SSS*XX + CCC*YY;
01100		EVL[4,1]	←	COTX + RX;
01200		EVL[4,2]	←	COTY + RY;
01300		EVL[2,2]	←	EVL[1,1]	←	CCW*RX/R;
01400		EVL[2,1]	←     -(EVL[1,2]	←	CCW*RY/R);
01500	END;
01600	
01700	INTERNAL BOOLEAN PROCEDURE DRIVE;
01800	BEGIN
01900		IF(I←I+1)>N THEN RETURN(FALSE);
02000		CAMERA_TO_VEHICLE(MCL,MVL);
02100		SUBDRIVE;
02200		VEHICLE_TO_CAMERA(EVL,ECL);
02350		ARRBLT(MCL[1,1],ECL[1,1],12);		α PERFECT VISION;
02351		RETURN(TRUE);
02400	END;
02500	
02600	END